/**
 * @file Joystick.c
 * @author SoBigDog (623644973lubin@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-11-16
 * 
 * @copyright Copyright (c) 2020
 * 
 */
#include <math.h>
#include "Joystick.h"

#include "SEGGER_RTT.h"

void JoystickInit(struct Joystick * pJoy)
{
	pJoy->Config.zero_offsetX = 2048;
	pJoy->Config.zero_offsetY = 2048;
	pJoy->Config.invalid_rangeY = 300;
	pJoy->Config.invalid_rangeY = 300;
	pJoy->Config.max_valueX = 0;
	pJoy->Config.min_valueX = 4095;
	pJoy->Config.max_valueY = 0;
	pJoy->Config.min_valueY = 4095;
}

bool JoystickUpdate(struct Joystick * pJoy)
{
	int32_t temp_sum_X;
	int32_t temp_sum_Y;
	int32_t temp_sum_value;
	bool hold_flag = false;

	temp_sum_X = 0;
	temp_sum_Y = 0;
	
	//(0, "%d %d %d %d %d %d\n", ADC_buff[0], ADC_buff[1], ADC_buff[2] ,ADC_buff[3], ADC_buff[4], ADC_buff[5]);

	for (int i = 0; i < 10; ++i) {
		temp_sum_X += ADC_buff[pJoy->u8ChannelX + 6 * i];
		temp_sum_Y += ADC_buff[pJoy->u8ChannelY + 6 * i];
	}

	pJoy->u16AverageX = temp_sum_X / 10 - pJoy->Config.zero_offsetX;
	pJoy->u16AverageY = (4096 - temp_sum_Y / 10) - pJoy->Config.zero_offsetY;
	temp_sum_value = (int32_t)pow(pJoy->u16AverageX * pJoy->u16AverageX + 
								   pJoy->u16AverageY * pJoy->u16AverageY, 0.5f);

	pJoy->u16VelotageX = pJoy->u16AverageX / 4096.0f * 3.3f;
	pJoy->u16VelotageY = pJoy->u16AverageY / 4096.0f * 3.3f;

	//if (((pJoy->u16AverageX < pJoy->Config.invalid_rangeX) 
	//	  && (pJoy->u16AverageX > -pJoy->Config.invalid_rangeX))
	//	 && (pJoy->u16AverageY < pJoy->Config.invalid_rangeY 
	//	  && pJoy->u16AverageY > -pJoy->Config.invalid_rangeY)) {
	//	
	//	hold_flag = true;
	//}

	if (temp_sum_value < 300) {
		hold_flag = true;
	}

	if (hold_flag) {
		pJoy->Vector.value = 0;
		pJoy->Vector.angle = PI / 2;
	} else {
		pJoy->Vector.value = temp_sum_value;
		pJoy->Vector.angle = atan2(pJoy->u16AverageY, pJoy->u16AverageX);
	}

	//SEGGER_RTT_printf(0, "%d %d %d %d\n", temp_sum_value, (uint32_t)(pJoy->Vector.value), pJoy->u16AverageX, pJoy->u16AverageY);

	return true;
}

